#include "motion.h"
#include "drive/motor.h"
#include "posture.h"
#include "drive/stream.h"
#include "drive/sys_timer.h"
#include "./drive/hc_sr04.h"
#include "face.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
#define ANGLE_MIN 2

#define MAX_TARGET MOTOR_MAX_OUT
#define AVOID_DISTANCE 50
#define AVOID_SPEED_RATE (MAX_TARGET / AVOID_DISTANCE)
#define AVOID_P (60)
#define AVOID_SPEED(distance) ((AVOID_DISTANCE - distance) * AVOID_SPEED_RATE * AVOID_P) 

int16_t target_angle = 0;
int16_t target_speed = 0;

int32_t target_l = 0;
int32_t target_r = 0;

typedef struct
{
  float Kp;
  float Ki;
  float Kd;
 
  float Ek;
  float Ek1;
  float Ek2;
  float Sum;
}PID_T;
 
PID_T angle_pid;
PID_T speed_pid_l;
PID_T speed_pid_r;

float MotionAnglePIDc(float diff) {
    PID_T *PID = &angle_pid;
    float PIDLoc;

    PID->Ek = diff;
    PID->Sum += PID->Ek;

    PIDLoc = PID->Kp * PID->Ek + (PID->Ki * PID->Sum) + PID->Kd * (PID->Ek1 - PID->Ek);

    PID->Ek1 = PID->Ek;
    return PIDLoc;
}

int MotionSpeedPid(int32_t diff, PID_T *pid) {
    int Pwm = 0;
    if (abs(diff) < 2) {
        Pwm = pid->Ek = 0;
    } else {
        // P I
        Pwm += pid->Kp * (diff - pid->Ek) + pid->Ki * diff;
        pid->Ek = diff;
    }
    return Pwm;
}

void MotionInit() {
    MotorInit();
    PostureInit();
    MotionRest();
    HcInit();
}

void MotionControl(int16_t angle, int16_t speed) {
    target_angle = angle < 360 ? angle : 0;
    target_speed = speed < 100 ? speed : 100;
}

float MotionGetAngle() {
    return PostureCurYaw();
}


int32_t MotionAngleOut(float diff) {
    int32_t out = 0;
    // if (abs(diff) > 180) {
    //     diff = (360 - abs(diff)) * (diff > 0 ? -1 : 1);
    // }
    if (abs(diff) < 2) {
      return 0;
    }
//    out = MotionAnglePIDc(diff);
    out = MotionSpeedPid(diff, &angle_pid);
    if (out > MAX_TARGET)
        out = MAX_TARGET;
    else if (out < -MAX_TARGET)
        out = -MAX_TARGET;
    return out >> 1;
}

char print_buf[50];

void MotionOut(int16_t l, int16_t r) {
  if (l > 0) {
      MotorOut(MOTOR_LEFT, MOTOR_FORWARD, l);
  } else {
      MotorOut(MOTOR_LEFT, MOTOR_BACKWARD, -l);
  }
  if (r > 0) {
      MotorOut(MOTOR_RIGHT, MOTOR_FORWARD, r);
  } else {
      MotorOut(MOTOR_RIGHT, MOTOR_BACKWARD, -r);
  }
}

void MotionSpeedProcess() {
    static uint32_t ctrl_time = CurMillis();
    static int32_t pwm_l=0, pwm_r=0;

    if ((ctrl_time + 10) > CurMillis()) 
        return;
    ctrl_time = CurMillis();
    volatile int diff_l = MotorGetSpeedL();
    volatile int diff_r = MotorGetSpeedR();

    pwm_l += MotionSpeedPid(target_l - diff_l, &speed_pid_l);
    pwm_r += MotionSpeedPid(target_r - diff_r, &speed_pid_r);

    MOTOR_LIMIT_OUT(pwm_l);
    MOTOR_LIMIT_OUT(pwm_r);
    MotionOut(pwm_l, pwm_r);
}

void MotionRest() {
  PostureReset();
  memset((char*)&angle_pid, 0, sizeof(angle_pid));
  memset((char*)&speed_pid_l, 0, sizeof(speed_pid_l));
  memset((char*)&speed_pid_r, 0, sizeof(speed_pid_r));
  angle_pid.Kp = 2;
  angle_pid.Ki = 1;
  angle_pid.Kd = 0;

  speed_pid_l.Kp = 15;
  speed_pid_l.Ki = 5;
  speed_pid_l.Kd = 0;

  speed_pid_r.Kp = 3;
  speed_pid_r.Ki = 1;
  speed_pid_r.Kd = 0;
  target_l = 10;
  target_r = 10;
}

void MotionRemoteProcess() {
  int32_t l,r;
  l = MOTOR_MAX_OUT * target_angle / 100;
  r = -l;
  l += MOTOR_MAX_OUT * target_speed / 100;
  r += MOTOR_MAX_OUT * target_speed / 100;
  MOTOR_LIMIT_OUT(l);
  MOTOR_LIMIT_OUT(r);
  if (l || r) {
    FaceSet((FACE_E)(2 + (CurMillis() % 3)));
  } else {
    FaceSet((FACE_E)(CurMillis() % 2));
  }
  MotionOut(l, r);
}

void MotionAvoidProcess() {
  int16_t speed_l=0, speed_r=0;
  static uint32_t ctrl_time = CurMillis();
  if (ctrl_time > CurMillis())
      return;
  ctrl_time = CurMillis() + 100;
  uint8_t * distance = HcGet();
  if (distance[HC_FRONT] < AVOID_DISTANCE) {
    speed_l = -AVOID_SPEED(distance[HC_FRONT]);
    speed_r = -AVOID_SPEED(distance[HC_FRONT]);
  }
  if (distance[HC_BACK] < AVOID_DISTANCE) {
    speed_l += AVOID_SPEED(distance[HC_BACK]);
    speed_r += AVOID_SPEED(distance[HC_BACK]);
  }

  if (distance[HC_LEFT] < AVOID_DISTANCE) {
    speed_l += AVOID_SPEED(distance[HC_LEFT]);
    speed_r -= AVOID_SPEED(distance[HC_LEFT]);
  }

  if (distance[HC_RIGHT] < AVOID_DISTANCE) {
    speed_l -= AVOID_SPEED(distance[HC_RIGHT]);
    speed_r += AVOID_SPEED(distance[HC_RIGHT]);
  }
  if (speed_l || speed_r) {
    FaceSet((FACE_E)(2 + (CurMillis() % 3)));
  } else {
    FaceSet((FACE_E)(CurMillis() % 2));
  }
  MotionOut(speed_l, speed_r);
}

void MotionStaticProcess() {
  static uint32_t ctrl_time = CurMillis();
  if (ctrl_time > CurMillis())
      return;
  ctrl_time = CurMillis() + 20;
  if (PostureStatus() == POSTRUE_OK) {
    int16_t cur_angle = MotionGetAngle();
    float diff = (target_angle + 540 - cur_angle) % 360 - 180;
    int32_t angle_out = MotionAngleOut(diff);
    MotionOut(angle_out, -angle_out);
  }
}
